{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "SparseTensorValue(indices=array([[0, 0],\n",
      "       [1, 1],\n",
      "       [1, 2],\n",
      "       [2, 1],\n",
      "       [2, 2]]), values=array([ 1.,  1., -0.,  0.,  1.], dtype=float32), dense_shape=array([3, 3]))\n",
      "SparseTensorValue(indices=array([[0, 0],\n",
      "       [0, 2],\n",
      "       [1, 1],\n",
      "       [2, 0],\n",
      "       [2, 2]]), values=array([ 1.,  0.,  1., -0.,  1.], dtype=float32), dense_shape=array([3, 3]))\n",
      "[[ 1.000000e+00  0.000000e+00  0.000000e+00  0.000000e+00]\n",
      " [ 0.000000e+00  7.962743e-04 -9.999997e-01  0.000000e+00]\n",
      " [ 0.000000e+00  9.999997e-01  7.962743e-04  0.000000e+00]\n",
      " [ 0.000000e+00  0.000000e+00  0.000000e+00  1.000000e+00]]\n",
      "[[1. 0. 0. 0.]\n",
      " [0. 1. 0. 0.]\n",
      " [0. 0. 1. 0.]\n",
      " [0. 0. 0. 1.]]\n",
      "[[500.   0. 300.]\n",
      " [  0. 500. 200.]\n",
      " [  0.   0.   1.]]\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import tensorflow as tf\n",
    "import scipy as sp\n",
    "\n",
    "#define data structures\n",
    "image_data = []\n",
    "imu_data = []\n",
    "state_data = []\n",
    "\n",
    "#camera intrinsics\n",
    "fx = tf.Variable(500.0)\n",
    "fy = tf.Variable(500.0)\n",
    "cx = tf.Variable(300.0)\n",
    "cy = tf.Variable(200.0)\n",
    "K = tf.sparse.SparseTensor(indices=[[0,0],[0,2],[1,1],[1,2],[2,2]],values=[fx,cx,fy,cy,1.0],dense_shape=[3,3])\n",
    "#imu intrinsics\n",
    "#camera_imu_extend\n",
    "r = tf.Variable(0.0)\n",
    "p = tf.Variable(0.0)\n",
    "y = tf.Variable(0.0)\n",
    "t = tf.Variable([[0.0],[0.0],[0.0]])\n",
    "C_r = tf.sparse.SparseTensor(indices=[[0,0],[1,1],[1,2],[2,1],[2,2]],values=[1.0,tf.math.cos(r),-tf.math.sin(r),tf.math.sin(r),tf.math.cos(r)],dense_shape=[3,3])\n",
    "C_p = tf.sparse.SparseTensor(indices=[[0,0],[0,2],[1,1],[2,0],[2,2]],values=[tf.math.cos(p),tf.math.sin(p),1.0,-tf.math.sin(p),tf.math.cos(p)],dense_shape=[3,3])\n",
    "C_y = tf.sparse.SparseTensor(indices=[[0,0],[0,1],[1,0],[1,1],[2,2]],values=[tf.math.cos(y),-tf.math.sin(y),tf.math.sin(y),tf.math.cos(y),1.0],dense_shape=[3,3])\n",
    "T_ir = tf.matmul(tf.matmul(tf.sparse_tensor_to_dense(C_y),tf.sparse_tensor_to_dense(C_p)),tf.sparse_tensor_to_dense(C_r))\n",
    "T_ir = tf.concat([T_ir,t],1)\n",
    "T_ir = tf.concat([T_ir,tf.constant([[0.0,0.0,0.0,1.0]])],0)\n",
    "r_c = tf.Variable(1.57)\n",
    "p_c = tf.Variable(0.0)\n",
    "y_c = tf.Variable(0.0)\n",
    "C_rc = tf.sparse.SparseTensor(indices=[[0,0],[1,1],[1,2],[2,1],[2,2]],values=[1.0,tf.math.cos(r_c),-tf.math.sin(r_c),tf.math.sin(r_c),tf.math.cos(r_c)],dense_shape=[3,3])\n",
    "C_pc = tf.sparse.SparseTensor(indices=[[0,0],[0,2],[1,1],[2,0],[2,2]],values=[tf.math.cos(p_c),tf.math.sin(p_c),1.0,-tf.math.sin(p_c),tf.math.cos(p_c)],dense_shape=[3,3])\n",
    "C_yc = tf.sparse.SparseTensor(indices=[[0,0],[0,1],[1,0],[1,1],[2,2]],values=[tf.math.cos(y_c),-tf.math.sin(y_c),tf.math.sin(y_c),tf.math.cos(y_c),1.0],dense_shape=[3,3])\n",
    "T_ci = tf.matmul(tf.matmul(tf.sparse_tensor_to_dense(C_yc),tf.sparse_tensor_to_dense(C_pc)),tf.sparse_tensor_to_dense(C_rc))\n",
    "T_ci = tf.concat([T_ci,t],1)\n",
    "T_ci = tf.concat([T_ci,tf.constant([[0.0,0.0,0.0,1.0]])],0)\n",
    "K_d = tf.sparse_tensor_to_dense(K)\n",
    "sess = tf.compat.v1.Session()\n",
    "init_op = tf.compat.v1.global_variables_initializer()\n",
    "sess.run(init_op)\n",
    "print(sess.run(C_r))\n",
    "print(sess.run(C_p))\n",
    "print(sess.run(T_ci))\n",
    "print(sess.run(T_ir))\n",
    "print(sess.run(K_d))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Tensor(\"truediv_11:0\", shape=(?, ?, 2), dtype=float32)\n"
     ]
    }
   ],
   "source": [
    "# place holder for measurements and states\n",
    "#time\n",
    "T = tf.compat.v1.placeholder(tf.int32,shape = [])\n",
    "J = tf.compat.v1.placeholder(tf.int32,shape = [])\n",
    "M = tf.compat.v1.placeholder(tf.int32,shape = [])\n",
    "#true states:\n",
    "true_a = tf.compat.v1.placeholder(tf.float32,shape = (None,3))\n",
    "true_w =  tf.compat.v1.placeholder(tf.float32,shape = (None,3))\n",
    "#true transformations:\n",
    "true_C_rw = tf.compat.v1.placeholder(tf.float32,shape = (None,3,3))\n",
    "true_T_rw = tf.compat.v1.placeholder(tf.float32,shape = (None,4,4))\n",
    "#gravity\n",
    "g_w = tf.compat.v1.placeholder(tf.float32,shape = [])\n",
    "#landmark\n",
    "p_w = tf.compat.v1.placeholder(tf.float32,shape = (None,4))\n",
    "#bias (state)\n",
    "b_a = tf.Variable([], validate_shape=False)\n",
    "b_w = tf.Variable([], validate_shape=False)\n",
    "#imu noise\n",
    "R_a = tf.placeholder(tf.float32,shape=(3,3))\n",
    "R_w = tf.placeholder(tf.float32,shape =(3,3))\n",
    "#measurements\n",
    "y_m = tf.compat.v1.placeholder(tf.float32,shape = (None,None,2))\n",
    "a_m = tf.compat.v1.placeholder(tf.float32,shape = (None,3))\n",
    "w_m = tf.compat.v1.placeholder(tf.float32,shape = (None,3))\n",
    "##estimations\n",
    "#acceleration\n",
    "flat_true_C_i = tf.reshape(true_C_iw, (-1,3))\n",
    "flat_true_a_m = tf.reshape(tf.matmul(flat_true_C_i,tf.transpose(true_a-g_w)),(-1,3))+b_a\n",
    "a_m_error = flat_true_a_m-a_m\n",
    "a_m_cost = tf.linalg.trace(tf.matmul(tf.matmul(a_m_error,R_a),tf.transpose(a_m_error)))\n",
    "#angular velocity\n",
    "flat_true_w_m = tf.reshape(tf.matmul(flat_true_C_i,tf.transpose(true_w)),(-1,3))+b_w\n",
    "w_m_error = flat_true_w_m-w_m\n",
    "w_m_cost = tf.linalg.trace(tf.matmul(tf.matmul(w_m_error,R_w),tf.transpose(w_m_error)))\n",
    "#reprojection\n",
    "p_i = tf.matmul(true_T_iw,tf.transpose(p_w))\n",
    "p_c = tf.matmul(tf.transpose(p_i,[0,2,1]),tf.transpose(T_ci))\n",
    "T_c_T = tf.concat([tf.eye(3,dtype=tf.float32),tf.zeros([1,3],tf.float32)],0)\n",
    "true_y_h= tf.matmul(tf.matmul(p_c,T_c_T),tf.transpose(K_d))\n",
    "true_y_m = tf.divide(true_y_h[:,:,0:2],true_y_h[:,:,2])\n",
    "print(true_y_m)\n",
    "y_m_error = "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[255 255 255 ... 255 246 249]\n",
      " [255 255 255 ... 247 255 255]\n",
      " [255 255 255 ...  16 148 255]\n",
      " ...\n",
      " [255 255 255 ... 255 255 255]\n",
      " [255 255 255 ... 255 255 255]\n",
      " [255 255 255 ... 255 255 255]]\n",
      "[]\n"
     ]
    }
   ],
   "source": [
    "import cv2\n",
    "import apriltag\n",
    "\n",
    "img = cv2.imread('april.jpeg',0)\n",
    "print(img)\n",
    "detector = apriltag.Detector()\n",
    "result = detector.detect(img)\n",
    "print(result)\n",
    "cv2.imshow('image',img)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import cv2\n",
    "import glob\n",
    "# termination criteria\n",
    "criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)\n",
    "\n",
    "# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)\n",
    "objp = np.zeros((6*7,3), np.float32)\n",
    "objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)\n",
    "\n",
    "# Arrays to store object points and image points from all the images.\n",
    "objpoints = [] # 3d point in real world space\n",
    "imgpoints = [] # 2d points in image plane.\n",
    "\n",
    "images = glob.glob('*.jpg')\n",
    "\n",
    "for fname in images:\n",
    "    img = cv2.imread(fname)\n",
    "    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)\n",
    "\n",
    "    # Find the chess board corners\n",
    "    ret, corners = cv2.findChessboardCorners(gray, (7,6),None)\n",
    "\n",
    "    # If found, add object points, image points (after refining them)\n",
    "    if ret == True:\n",
    "        objpoints.append(objp)\n",
    "\n",
    "        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)\n",
    "        imgpoints.append(corners2)\n",
    "\n",
    "        # Draw and display the corners\n",
    "        img = cv2.drawChessboardCorners(img, (7,6), corners2,ret)\n",
    "        cv2.imshow('img',img)\n",
    "        cv2.waitKey(500)\n",
    "\n",
    "cv2.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "['index1.jpg', 'index1 (copy).jpg', 'calib_pattern.jpg', 'index.jpg']\n",
      "[179.38551 179.38715 179.39127]\n"
     ]
    }
   ],
   "source": [
    "print(images)\n",
    "print(np.max(np.asarray(imgpoints).reshape((-1,3)),axis=0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "179.38551\n",
      "179.38715\n",
      "[[155.50456   35.45695  155.50603 ]\n",
      " [ 59.492615 155.50597   83.41682 ]\n",
      " [155.50362  107.426    155.50261 ]\n",
      " [131.42165  155.50293  155.36176 ]\n",
      " [155.50533  179.38715  131.51321 ]\n",
      " [ 35.453564 131.51111   59.490196]\n",
      " [131.5109    83.41406  131.51549 ]\n",
      " [107.42185  131.50717  131.41592 ]\n",
      " [131.51888  155.3596   131.51335 ]\n",
      " [179.37946  107.49776   35.457188]\n",
      " [107.49963   59.494373 107.497284]\n",
      " [ 83.41861  107.50053  107.433495]\n",
      " [107.49707  131.42744  107.49827 ]\n",
      " [155.36656  107.49934  179.39127 ]\n",
      " [ 83.483826  35.45153   83.48429 ]\n",
      " [ 59.490486  83.483635  83.41224 ]\n",
      " [ 83.48761  107.42182   83.484665]\n",
      " [131.41637   83.49198  155.36066 ]\n",
      " [ 83.48788  179.38045   59.501244]\n",
      " [ 35.452023  59.500084  59.49359 ]\n",
      " [ 59.50057   83.41248   59.500355]\n",
      " [107.43131   59.499207 131.4193  ]\n",
      " [ 59.498898 155.36526   59.50042 ]\n",
      " [179.38551   35.479362  35.44871 ]\n",
      " [ 35.483818  59.49008   35.48099 ]\n",
      " [ 83.41172   35.48924  107.422676]\n",
      " [ 35.482693 131.41776   35.492374]\n",
      " [155.36038   35.48803  179.38124 ]\n",
      " [155.50456   35.45695  155.50603 ]\n",
      " [ 59.492615 155.50597   83.41682 ]\n",
      " [155.50362  107.426    155.50261 ]\n",
      " [131.42165  155.50293  155.36176 ]\n",
      " [155.50533  179.38715  131.51321 ]\n",
      " [ 35.453564 131.51111   59.490196]\n",
      " [131.5109    83.41406  131.51549 ]\n",
      " [107.42185  131.50717  131.41592 ]\n",
      " [131.51888  155.3596   131.51335 ]\n",
      " [179.37946  107.49776   35.457188]\n",
      " [107.49963   59.494373 107.497284]\n",
      " [ 83.41861  107.50053  107.433495]\n",
      " [107.49707  131.42744  107.49827 ]\n",
      " [155.36656  107.49934  179.39127 ]\n",
      " [ 83.483826  35.45153   83.48429 ]\n",
      " [ 59.490486  83.483635  83.41224 ]\n",
      " [ 83.48761  107.42182   83.484665]\n",
      " [131.41637   83.49198  155.36066 ]\n",
      " [ 83.48788  179.38045   59.501244]\n",
      " [ 35.452023  59.500084  59.49359 ]\n",
      " [ 59.50057   83.41248   59.500355]\n",
      " [107.43131   59.499207 131.4193  ]\n",
      " [ 59.498898 155.36526   59.50042 ]\n",
      " [179.38551   35.479362  35.44871 ]\n",
      " [ 35.483818  59.49008   35.48099 ]\n",
      " [ 83.41172   35.48924  107.422676]\n",
      " [ 35.482693 131.41776   35.492374]\n",
      " [155.36038   35.48803  179.38124 ]]\n"
     ]
    }
   ],
   "source": [
    "img_flat = np.reshape(np.asarray(imgpoints),(-1,3))\n",
    "max_x = np.max(img_flat,axis = 0)[0]\n",
    "max_y = np.max(img_flat,axis = 0)[1]\n",
    "min_x = np.min(img_flat,axis = 0)[0]\n",
    "min_y = np.min(img_flat,axis = 0)[1]\n",
    "print(max_x)\n",
    "print(max_y)\n",
    "print(img_flat)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0 1]\n"
     ]
    },
    {
     "ename": "NameError",
     "evalue": "name 'criteria_cal' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                              Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-36-0fbfa0373755>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m      8\u001b[0m     \u001b[0mcur_obj\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mlist\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mobjpoints\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mi\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;32mfor\u001b[0m \u001b[0mi\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mcur_data\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      9\u001b[0m     \u001b[0mcur_img\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mlist\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mimgpoints\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mi\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;32mfor\u001b[0m \u001b[0mi\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mcur_data\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 10\u001b[0;31m     \u001b[0mret\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mmtx\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mdist\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mrvecs\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtvecs\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mcv2\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcalibrateCamera\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mcur_obj\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mcur_img\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mgray\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mshape\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0mcameraMatrix\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mmtx\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mcriteria\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mcriteria_cal\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m: name 'criteria_cal' is not defined"
     ]
    }
   ],
   "source": [
    "list1 = np.arange(0,len(imgpoints),1)\n",
    "mtx = np.zeros((3,3))\n",
    "for i in range(3):\n",
    "    cur_data = list1\n",
    "    print(list1)\n",
    "    if len(imgpoints)>20:\n",
    "        cur_data = sample(list,20)\n",
    "    cur_obj = list(objpoints[i] for i in cur_data)\n",
    "    cur_img = list(imgpoints[i] for i in cur_data)\n",
    "    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(cur_obj, cur_img, gray.shape[::-1],cameraMatrix=mtx, criteria=criteria_cal)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
